//==================================================================//
//==================================================================//
//= Math.cpp =======================================================//
//==================================================================//
//= Original coder: Trent Polack (ShiningKnight) ===================//
//= Code modifications by: Christian Ofenberg (AblazeSpace) ========//
//==================================================================//
//==================================================================//


//------------------------------------------------------------------//
//------------------------------------------------------------------//
//- INCLUDES -------------------------------------------------------//
//------------------------------------------------------------------//
//------------------------------------------------------------------//
#include "Shining3D.h"


//------------------------------------------------------------------//
//------------------------------------------------------------------//
//- DEFINITIONS ----------------------------------------------------//
//------------------------------------------------------------------//
//------------------------------------------------------------------//

//------------------------------------------------------------------//
//- GLint Random(GLint, GLint) -------------------------------------//
//------------------------------------------------------------------//
//- Description: Returns a random number from the range of numbers -//
//-              that you supply.								   -//
//------------------------------------------------------------------//
GLint Random(GLint min, GLint max) 
	{ return (rand()%(max-min+1))+min; }


//------------------------------------------------------------------//
//- void ComputeNormal(VECTOR3D, VECTOR3D, VECTOR3D) ---------------//
//------------------------------------------------------------------//
//- Description: Sends a surface normal (composed of 3 vectors), to-//
//-              OpenGL.										   -//
//------------------------------------------------------------------//
void ComputeNormal(VECTOR3D v1, VECTOR3D v2, VECTOR3D v3)
	{
	VECTOR3D result = ((v1-v2).CrossProduct((v1-v3))).Normalize();

	//Specify the normal
	glNormal3fv(result.v);
	}

//------------------------------------------------------------------//
//- void VERTEX::SendToOGL(void) -----------------------------------//
//------------------------------------------------------------------//
//- Description: This function sends the class's vertex array to   -//
//-				 OpenGL.										   -//
//------------------------------------------------------------------//
float VECTOR::
	ComputeLength(void)
	{
	float length;

	// calculate the length of the normal
	length= (float)sqrt(SQUARE(vector[0]) + 
						SQUARE(vector[1]) + 
						SQUARE(vector[2]));

	return length;
	}

//------------------------------------------------------------------//
//- float ComputeDotProduct(VECTOR*, VECTOR*) ----------------------//
//------------------------------------------------------------------//
//- Description: Returns the dot product of two vectors (this stuff-//
//-              is pretty self explanatory, eh?).				   -//
//------------------------------------------------------------------//
float ComputeDotProduct(VECTOR* v1, VECTOR* v2)
	{
	float dot;

	dot= (v1->vector[0]+v2->vector[0])+
		 (v1->vector[1]+v2->vector[1])+
		 (v1->vector[2]+v2->vector[2]);

	return dot;
	}

//------------------------------------------------------------------//
//- VECTOR ComputeCrossProduct(VECTOR*, VECTOR*) -------------------//
//------------------------------------------------------------------//
//- Description: Returns the cross product of two vectors.		   -//
//------------------------------------------------------------------//
VECTOR ComputeCrossProduct(const VECTOR &v1, const VECTOR &v2)
	{
	VECTOR result(0.0f, 0.0f, 0.0f);

	result.vector[0]= (v1.vector[1]*v2.vector[2]) - (v1.vector[2]*v2.vector[1]);
	result.vector[1]= (v1.vector[2]*v2.vector[0]) - (v1.vector[0]*v2.vector[2]);
	result.vector[2]= (v1.vector[0]*v2.vector[1]) - (v1.vector[1]*v2.vector[0]);

	return result;
	}

//------------------------------------------------------------------//
//- VECTOR(float, float, float) ------------------------------------//
//------------------------------------------------------------------//
//- Description: The constructor that intializes the parts of the  -//
//-				 vector to your specifications.					   -//
//------------------------------------------------------------------//
VECTOR::
	VECTOR(float x, float y, float z)
	{
	vector[0]=x;
	vector[1]=y;
	vector[2]=z;
	}

//------------------------------------------------------------------//
//- inline VECTOR VECTOR::operator+ (VECTOR) -----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the + operator, allowing you to add   -//
//-				 one vector to another, with some nice looking     -//
//-				 code.											   -//
//------------------------------------------------------------------//
inline VECTOR VECTOR::
	operator+ (const VECTOR &v)
	{
	VECTOR result(0.0f, 0.0f, 0.0f);

	result.vector[0]= vector[0] + v.vector[0];
	result.vector[1]= vector[1] + v.vector[1];
	result.vector[2]= vector[2] + v.vector[2];

	return result;
	}

//------------------------------------------------------------------//
//- inline VECTOR VECTOR::operator- (VECTOR) -----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the - operator, allowing you to	   -//
//-				 subtract one vector to another, with some nice	   -//
//-				 looking code.									   -//
//------------------------------------------------------------------//
inline VECTOR VECTOR::
	operator- (const VECTOR &v)
	{
	VECTOR result(0.0f, 0.0f, 0.0f);

	result.vector[0]= vector[0] - v.vector[0];
	result.vector[1]= vector[1] - v.vector[1];
	result.vector[2]= vector[2] - v.vector[2];

	return result;
	}


//------------------------------------------------------------------//
//- inline VECTOR VECTOR::operator* (float) ------------------------//
//------------------------------------------------------------------//
//- Description: Overloading the * operator, this allows you to    -//
//-				 multiply a every component of a vector by a scalar-//
//-				 (single value).								   -//
//------------------------------------------------------------------//
inline VECTOR VECTOR::
	operator* (const float scalar)
	{
	VECTOR result(0.0f, 0.0f, 0.0f);

	result.vector[0]= vector[0] * scalar;
	result.vector[1]= vector[1] * scalar;
	result.vector[2]= vector[2] * scalar;

	return result;
	}

//------------------------------------------------------------------//
//- inline VECTOR VECTOR::operator* (VECTOR) -----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the * operator.  But this time it is  -//
//-				 for vector by vector multiplication.			   -//
//------------------------------------------------------------------//
inline VECTOR VECTOR::
	operator* (const VECTOR &v)
	{
	VECTOR result(0.0f, 0.0f, 0.0f);

	result.vector[0]= vector[0] * v.vector[0];
	result.vector[1]= vector[1] * v.vector[1];
	result.vector[2]= vector[2] * v.vector[2];

	return result;
	}

//------------------------------------------------------------------//
//- inline VECTOR VECTOR::operator/ (VECTOR) -----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the / operator, allowing you to divide-//
//-				 one vector to another.							   -//
//------------------------------------------------------------------//
inline VECTOR VECTOR::
	operator/ (const VECTOR &v)
	{
	VECTOR result(0.0f, 0.0f, 0.0f);

	result.vector[0]= vector[0] / v.vector[0];
	result.vector[1]= vector[1] / v.vector[1];
	result.vector[2]= vector[2] / v.vector[2];

	return result;
	}


//------------------------------------------------------------------//
//- VERTEX(float, float, float) ------------------------------------//
//------------------------------------------------------------------//
//- Description: The constructor that intializes the parts of the  -//
//-				 vector to your specifications.					   -//
//------------------------------------------------------------------//
VERTEX::
	VERTEX(float x, float y, float z)
	{
	vertex[0]=x;
	vertex[1]=y;
	vertex[2]=z;
	}

//------------------------------------------------------------------//
//- void VERTEX::SendToOGL(void) -----------------------------------//
//------------------------------------------------------------------//
//- Description: This function sends the class's vertex array to   -//
//-				 OpenGL.										   -//
//------------------------------------------------------------------//
void VERTEX::
	SendToOGL(void)
	{	glVertex3fv(vertex);	}

//------------------------------------------------------------------//
//- inline VERTEX VERTEX::operator+ (VERTEX) -----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the + operator, allowing you to add   -//
//-				 one vertex to another, with some nice looking     -//
//-				 code.											   -//
//------------------------------------------------------------------//
inline VERTEX VERTEX::
	operator+ (const VERTEX &v)
	{
	VERTEX result(0.0f, 0.0f, 0.0f);

	result.vertex[0]= vertex[0] + v.vertex[0];
	result.vertex[1]= vertex[1] + v.vertex[1];
	result.vertex[2]= vertex[2] + v.vertex[2];

	return result;
	}

//------------------------------------------------------------------//
//- inline VERTEX VERTEX::operator- (VERTEX) -----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the - operator, allowing you to	   -//
//-				 subtract one vertex to another, with some nice	   -//
//-				 looking code.									   -//
//------------------------------------------------------------------//
inline VERTEX VERTEX::
	operator- (const VERTEX &v)
	{
	VERTEX result(0.0f, 0.0f, 0.0f);

	result.vertex[0]= vertex[0] - v.vertex[0];
	result.vertex[1]= vertex[1] - v.vertex[1];
	result.vertex[2]= vertex[2] - v.vertex[2];

	return result;
	}


//------------------------------------------------------------------//
//- inline VECTOR VECTOR::operator* (float) ------------------------//
//------------------------------------------------------------------//
//- Description: Overloading the * operator, this allows you to    -//
//-				 multiply a every component of a vertex by a scalar-//
//-				 (single value).								   -//
//------------------------------------------------------------------//
inline VERTEX VERTEX::
	operator* (const float scalar)
	{
	VERTEX result(0.0f, 0.0f, 0.0f);

	result.vertex[0]= vertex[0] * scalar;
	result.vertex[1]= vertex[1] * scalar;
	result.vertex[2]= vertex[2] * scalar;

	return result;
	}

//------------------------------------------------------------------//
//- inline VERTEX VERTEX::operator* (VERTEX) -----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the * operator.  But this time it is  -//
//-				 for vertex by vertex multiplication.			   -//
//------------------------------------------------------------------//
inline VERTEX VERTEX::
	operator* (const VERTEX &v)
	{
	VERTEX result(0.0f, 0.0f, 0.0f);

	result.vertex[0]= vertex[0] * v.vertex[0];
	result.vertex[1]= vertex[1] * v.vertex[1];
	result.vertex[2]= vertex[2] * v.vertex[2];

	return result;
	}

//------------------------------------------------------------------//
//- inline VERTEX VERTEX::operator/ (VERTEX) -----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the / operator, allowing you to divide-//
//-				 one vertex to another.							   -//
//------------------------------------------------------------------//
inline VERTEX VERTEX::
	operator/ (const VERTEX &v)
	{
	VERTEX result(0.0f, 0.0f, 0.0f);

	result.vertex[0]= vertex[0] / v.vertex[0];
	result.vertex[1]= vertex[1] / v.vertex[1];
	result.vertex[2]= vertex[2] / v.vertex[2];

	return result;
	}




// MATRIX4X4 functions ***********************************************

MATRIX4X4 MATRIX4X4::operator*(const MATRIX4X4 m2)
{
	MATRIX4X4 mT;

	mT.m[0] = m[0]*m2.m[0] + m[4]*m2.m[1] + m[8]*m2.m[2] + m[12]*m2.m[3];
	mT.m[1] = m[1]*m2.m[0] + m[5]*m2.m[1] + m[9]*m2.m[2] + m[13]*m2.m[3];
	mT.m[2] = m[2]*m2.m[0] + m[6]*m2.m[1] + m[10]*m2.m[2] + m[14]*m2.m[3];
	mT.m[3] = m[3]*m2.m[0] + m[7]*m2.m[1] + m[11]*m2.m[2] + m[15]*m2.m[3];
	mT.m[4] = m[0]*m2.m[4] + m[4]*m2.m[5] + m[8]*m2.m[6] + m[12]*m2.m[7];
	mT.m[5] = m[1]*m2.m[4] + m[5]*m2.m[5] + m[9]*m2.m[6] + m[13]*m2.m[7];
	mT.m[6] = m[2]*m2.m[4] + m[6]*m2.m[5] + m[10]*m2.m[6] + m[14]*m2.m[7];
	mT.m[7] = m[3]*m2.m[4] + m[7]*m2.m[5] + m[11]*m2.m[6] + m[15]*m2.m[7];
	mT.m[8] = m[0]*m2.m[8] + m[4]*m2.m[9] + m[8]*m2.m[10] + m[12]*m2.m[11];
	mT.m[9] = m[1]*m2.m[8] + m[5]*m2.m[9] + m[9]*m2.m[10] + m[13]*m2.m[11];
	mT.m[10] = m[2]*m2.m[8] + m[6]*m2.m[9] + m[10]*m2.m[10] + m[14]*m2.m[11];
	mT.m[11] = m[3]*m2.m[8] + m[7]*m2.m[9] + m[11]*m2.m[10] + m[15]*m2.m[11];
	mT.m[12] = m[0]*m2.m[12] + m[4]*m2.m[13] + m[8]*m2.m[14] + m[12]*m2.m[15];
	mT.m[13] = m[1]*m2.m[12] + m[5]*m2.m[13] + m[9]*m2.m[14] + m[13]*m2.m[15];
	mT.m[14] = m[2]*m2.m[12] + m[6]*m2.m[13] + m[10]*m2.m[14] + m[14]*m2.m[15];
	mT.m[15] = m[3]*m2.m[12] + m[7]*m2.m[13] + m[11]*m2.m[14] + m[15]*m2.m[15];
	
	return mT;
}

VECTOR3D MATRIX4X4::operator*(const VECTOR3D vT)
{
	return VECTOR3D(vT.x*xx + vT.y*xy + vT.z*xz+xw,
					vT.x*yx + vT.y*yy + vT.z*yz+yw,
					vT.x*zx + vT.y*zy + vT.z*zz+zw);
}

//------------------------------------------------------------------//
//- MATRIX4X4 operator*(MATRIX4X4, float) --------------------------//
//------------------------------------------------------------------//
//- Description: Overloading the * operator.  This is for when you -//
//-				 want to multiply every component of a matrix by a -//
//-				 a scalar.										   -//
//------------------------------------------------------------------//
MATRIX4X4 operator*(const MATRIX4X4 &m1, const float scalar)
	{
	MATRIX4X4 mT;

	mT.m[0]= m1.m[0]*scalar;
	mT.m[1]= m1.m[1]*scalar;
	mT.m[2]= m1.m[2]*scalar;
	mT.m[3]= m1.m[3]*scalar;
	mT.m[4]= m1.m[4]*scalar;
	mT.m[5]= m1.m[5]*scalar;
	mT.m[6]= m1.m[6]*scalar;
	mT.m[7]= m1.m[7]*scalar;
	mT.m[8]= m1.m[8]*scalar;
	mT.m[9]= m1.m[9]*scalar;
	mT.m[10]=m1.m[10]*scalar;
	mT.m[11]=m1.m[11]*scalar;
	mT.m[12]=m1.m[12]*scalar;
	mT.m[13]=m1.m[13]*scalar;
	mT.m[14]=m1.m[14]*scalar;
	mT.m[15]=m1.m[15]*scalar;

	return mT;
	}

//------------------------------------------------------------------//
//- MATRIX4X4 operator+(MATRIX4X4, MATRIX4X4) ----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the + operator.  This adds the        -//
//-				 contents of one matrix to another, and returns the-//
//-				 results.										   -//
//------------------------------------------------------------------//
MATRIX4X4 operator+(const MATRIX4X4 &m1, const MATRIX4X4 &m2)
	{
	MATRIX4X4 mT;

	mT.m[0] = m1.m[0] + m2.m[0];
	mT.m[1] = m1.m[1] + m2.m[1];
	mT.m[2] = m1.m[2] + m2.m[2];
	mT.m[3] = m1.m[3] + m2.m[3];
	mT.m[4] = m1.m[4] + m2.m[4];
	mT.m[5] = m1.m[5] + m2.m[5];
	mT.m[6] = m1.m[6] + m2.m[6];
	mT.m[7] = m1.m[7] + m2.m[7];
	mT.m[8] = m1.m[8] + m2.m[8];
	mT.m[9] = m1.m[9] + m2.m[9];
	mT.m[10] =m1.m[10] + m2.m[10];
	mT.m[11] =m1.m[11] + m2.m[11];
	mT.m[12] =m1.m[12] + m2.m[12];
	mT.m[13] =m1.m[13] + m2.m[13];
	mT.m[14] =m1.m[14] + m2.m[14];
	mT.m[15] =m1.m[15] + m2.m[15];

	return mT;
	}

//------------------------------------------------------------------//
//- MATRIX4X4 operator-(MATRIX4X4, MATRIX4X4) ----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the - operator.  This subtracts the   -//
//-				 contents of one matrix to another, and returns the-//
//-				 results.										   -//
//------------------------------------------------------------------//
MATRIX4X4 operator-(const MATRIX4X4 &m1, const MATRIX4X4 &m2)
	{
	MATRIX4X4 mT;

	mT.m[0] = m1.m[0] - m2.m[0];
	mT.m[1] = m1.m[1] - m2.m[1];
	mT.m[2] = m1.m[2] - m2.m[2];
	mT.m[3] = m1.m[3] - m2.m[3];
	mT.m[4] = m1.m[4] - m2.m[4];
	mT.m[5] = m1.m[5] - m2.m[5];
	mT.m[6] = m1.m[6] - m2.m[6];
	mT.m[7] = m1.m[7] - m2.m[7];
	mT.m[8] = m1.m[8] - m2.m[8];
	mT.m[9] = m1.m[9] - m2.m[9];
	mT.m[10] = m1.m[10] - m2.m[10];
	mT.m[11] = m1.m[11] - m2.m[11];
	mT.m[12] = m1.m[12] - m2.m[12];
	mT.m[13] = m1.m[13] - m2.m[13];
	mT.m[14] = m1.m[14] - m2.m[14];
	mT.m[15] = m1.m[15] - m2.m[15];

	return mT;
	}

//------------------------------------------------------------------//
//- MATRIX4X4 operator*(MATRIX4X4, MATRIX4X4) ----------------------//
//------------------------------------------------------------------//
//- Description: Overloading the * operator.  This does the 'real' -//
//-				 matrix multiplication, and is pretty slow, though -//
//-				 not nearly as slow as the division operation      -//
//-				 though.										   -//
//------------------------------------------------------------------//
MATRIX4X4 operator*(const MATRIX4X4 &m1, const MATRIX4X4 &m2)
	{
	MATRIX4X4 mT;

	mT.m[0] = (m1.m[0]*m2.m[0]) + (m1.m[4]*m2.m[1]) + (m1.m[8]*m2.m[2]) + (m1.m[12]*m2.m[3]);
	mT.m[4] = (m1.m[0]*m2.m[4]) + (m1.m[4]*m2.m[5]) + (m1.m[8]*m2.m[6]) + (m1.m[12]*m2.m[7]);
	mT.m[8] = (m1.m[0]*m2.m[8]) + (m1.m[4]*m2.m[9]) + (m1.m[8]*m2.m[10]) + (m1.m[12]*m2.m[11]);
	mT.m[12] = (m1.m[0]*m2.m[12]) + (m1.m[4]*m2.m[13]) + (m1.m[8]*m2.m[14]) + (m1.m[12]*m2.m[15]);
	
	mT.m[1] = (m1.m[1]*m2.m[0]) + (m1.m[5]*m2.m[1]) + (m1.m[9]*m2.m[2]) + (m1.m[13]*m2.m[3]);
	mT.m[5] = (m1.m[1]*m2.m[4]) + (m1.m[5]*m2.m[5]) + (m1.m[9]*m2.m[6]) + (m1.m[13]*m2.m[7]);
	mT.m[9] = (m1.m[1]*m2.m[8]) + (m1.m[5]*m2.m[9]) + (m1.m[9]*m2.m[10]) + (m1.m[13]*m2.m[11]);
	mT.m[13] = (m1.m[1]*m2.m[12]) + (m1.m[5]*m2.m[13]) + (m1.m[9]*m2.m[14]) + (m1.m[13]*m2.m[15]);

	mT.m[2] = (m1.m[2]*m2.m[0]) + (m1.m[6]*m2.m[1]) + (m1.m[10]*m2.m[2]) + (m1.m[14]*m2.m[3]);
	mT.m[6] = (m1.m[2]*m2.m[4]) + (m1.m[6]*m2.m[5]) + (m1.m[10]*m2.m[6]) + (m1.m[14]*m2.m[7]);
	mT.m[10] = (m1.m[2]*m2.m[8]) + (m1.m[6]*m2.m[9]) + (m1.m[10]*m2.m[10]) + (m1.m[14]*m2.m[11]);
	mT.m[14] = (m1.m[2]*m2.m[12]) + (m1.m[6]*m2.m[13]) + (m1.m[10]*m2.m[14]) + (m1.m[14]*m2.m[15]);

	mT.m[3] = (m1.m[3]*m2.m[0]) + (m1.m[7]*m2.m[1]) + (m1.m[11]*m2.m[2]) + (m1.m[15]*m2.m[3]);
	mT.m[7] = (m1.m[3]*m2.m[4]) + (m1.m[7]*m2.m[5]) + (m1.m[11]*m2.m[6]) + (m1.m[15]*m2.m[7]);
	mT.m[11] = (m1.m[3]*m2.m[8]) + (m1.m[7]*m2.m[9]) + (m1.m[11]*m2.m[10]) + (m1.m[15]*m2.m[11]);
	mT.m[15] = (m1.m[3]*m2.m[12]) + (m1.m[7]*m2.m[13]) + (m1.m[11]*m2.m[14]) + (m1.m[15]*m2.m[15]);

	return mT;
	}

void MATRIX4X4::Transpose(void)
	{
    SWAP(xy, yx);
    SWAP(xz, zx);
    SWAP(xw, wx);
    SWAP(yz, zy);
    SWAP(yw, wy);
    SWAP(zw, wz);
	}

void MATRIX4X4::GetEulerAngles(float &xT, float &yT, float &zT)
	{
	double angleX, angleY, angleZ;

	angleY = atan2(zx, sqrt(zy*zy+zz*zz));
	double cosangleY = cos(angleY);

	if(fabs(cosangleY) > EPSILON)
	{
		angleZ = atan2(-yx/cosangleY, xx/cosangleY);
		angleX = atan2(-zy/cosangleY, zz/cosangleY);
	}
	else
	{
		if(fabs(PId2-angleY) < EPSILON)
		{
			angleX = atan2(xy , yy);
			angleY = PId2;
			angleZ = 0.0;
		}
		else
		{
			angleX = atan2(-xy, yy);
			angleY = -PId2;
			angleZ = 0.0;
		}
	}

	// We set the result:
	xT = (float) (-angleX*PIUNDER180);
	yT = (float) (-angleY*PIUNDER180);
	zT = (float) (-angleZ*PIUNDER180);
	}




// QUATERNION functions **********************************************

QUATERNION::QUATERNION(void)
	{ Reset(); }

QUATERNION::QUATERNION(const QUATERNION &copyFrom)
	{ Set(copyFrom); }

QUATERNION::QUATERNION(float xT, float yT, float zT)
	{ Set(xT, yT, zT); }

QUATERNION::QUATERNION(float angle, float xT, float yT, float zT)
	{ Set(angle, xT, yT, zT); }

QUATERNION &QUATERNION::Reset(void)
	{
	q[0] = 1.0f;
	q[1] = 0.0f;
	q[2] = 0.0f;
	q[3] = 0.0f;

	return *this;
	}

QUATERNION &QUATERNION::Set(const QUATERNION &copyFrom)
	{
	q[0] = copyFrom.q[0];
	q[1] = copyFrom.q[1];
	q[2] = copyFrom.q[2];
	q[3] = copyFrom.q[3];
	
	return *this;
	}

QUATERNION &QUATERNION::Set(float xT, float yT, float zT)
	{
	QUATERNION xQ(xT, 1.0f, 0.0f, 0.0f);
	QUATERNION yQ(yT, 0.0f, 1.0f, 0.0f);
	QUATERNION zQ(zT, 0.0f, 0.0f, 1.0f);

	Set(xQ);
	PostMult(yQ);
	PostMult(zQ);
	
	return *this;
	}

QUATERNION &QUATERNION::Set(float angle, float xT, float yT, float zT)
	{
	float factor = xT*xT+yT*yT+zT*zT;
	if(!factor)
		factor = (float) EPSILON;

	float scaleBy = (float) (1.0/sqrt(factor));

	q[0] = (float) cos(angle/2.0f);

	float sinHalfAngle = (float) sin(angle/2.0f);
	q[1] = xT*scaleBy*sinHalfAngle;
	q[2] = yT*scaleBy*sinHalfAngle;
	q[3] = zT*scaleBy*sinHalfAngle;

	return *this;
	}

QUATERNION &QUATERNION::PostMult(const QUATERNION &quat)
	{
	QUATERNION tempQ(*this);
	MultAndSet(tempQ, quat);

	return *this;
	}

QUATERNION &QUATERNION::MultAndSet(const QUATERNION &quat1, const QUATERNION &quat2)
{
	q[0] = quat2.q[0]*quat1.q[0]
		   -quat2.q[1]*quat1.q[1]
		   -quat2.q[2]*quat1.q[2]
		   -quat2.q[3]*quat1.q[3];

	q[1] = quat2.q[0]*quat1.q[1]
		   +quat2.q[1]*quat1.q[0]
		   +quat2.q[2]*quat1.q[3]
		   -quat2.q[3]*quat1.q[2];

	q[2] = quat2.q[0]*quat1.q[2]
		   -quat2.q[1]*quat1.q[3]
		   +quat2.q[2]*quat1.q[0]
		   +quat2.q[3]*quat1.q[1];

	q[3] = quat2.q[0]*quat1.q[3]
		   +quat2.q[1]*quat1.q[2]
		   -quat2.q[2]*quat1.q[1]
		   +quat2.q[3]*quat1.q[0];

	return *this;
	}

QUATERNION &QUATERNION::Normalize(void)
	{
	float factor = q[0]*q[0]+
				   q[1]*q[1]+
				   q[2]*q[2]+
				   q[3]*q[3];
	float scaleBy = (float) (1.0/sqrt(factor));

	q[0] = q[0]*scaleBy;
	q[1] = q[1]*scaleBy;
	q[2] = q[2]*scaleBy;
	q[3] = q[3]*scaleBy;

	return *this;
	}

QUATERNION QUATERNION::Inversed(void)
	{
	QUATERNION qT;
	
	qT.q[1] = -q[1];
	qT.q[2] = -q[2];
	qT.q[3] = -q[3];

	return qT;
	}

float QUATERNION::DotProduct(void)
	{ return x*x+y*y+z*z+w*w; }

float QUATERNION::DotProduct(QUATERNION qT)
	{ return x*qT.x+y*qT.y+z*qT.z+w*qT.w; }

float QUATERNION::GetLength(void)
	{ return (float) sqrt(x*x+y*y+z*z+w*w); }

void QUATERNION::GetMatrix(MATRIX4X4 *mT)
	{
	Normalize();

	float xx = x*x;
	float yy = y*y;
	float zz = z*z;


	mT->xx = 1.0f-2.0f*(yy+zz);
	mT->yx = 2.0f*(x*y+w*z);
	mT->zx = 2.0f*(x*z-w*y);
	mT->wx = 0.0f;

	mT->xy = 2.0f*(x*y-w*z);
	mT->yy = 1.0f-2.0f*(xx+zz);
	mT->zy = 2.0f*(y*z+w*x);
	mT->wy= 0.0f;

	mT->xz = 2.0f*(x*z+w*y);
	mT->yz = 2.0f*(y*z-w*x);
	mT->zz = 1.0f-2.0f*(xx+yy);
	mT->wz = 0.0f;

	mT->xw = 0.0f;
	mT->yw = 0.0f;
	mT->zw = 0.0f;
	mT->ww = 1.0f;
	}

void QUATERNION::GetInvertedMatrix(MATRIX4X4 *mT)
	{
	Normalize();

	float xx = x*x;
	float yy = y*y;
	float zz = z*z;

	mT->xx = -(1.0f-2.0f*(yy+zz));
	mT->yx = -(2.0f*(x*y+w*z));
	mT->zx = -(2.0f*(x*z-w*y));
	mT->wx = 0.0f;

	mT->xy = 2.0f*(x*y-w*z);
	mT->yy = 1.0f-2.0f*(xx+zz);
	mT->yz = 2.0f*(y*z+w*x);
	mT->yw = 0.0f;

	mT->zx = 2.0f*(x*z+w*y);
	mT->zy = 2.0f*(y*z-w*x);
	mT->zz = 1.0f-2.0f*(xx+yy);
	mT->zw = 0.0f;

	mT->wx = 0.0f;
	mT->wy = 0.0f;
	mT->wz = 0.0f;
	mT->ww = 1.0f;
	}

void QUATERNION::SetMatrix(MATRIX4X4 mT)
	{
	float tr, s, q[4];
    int i, j, k;
	int nxt[3] = {1, 2, 0};

	tr = mT.xx+mT.yy+mT.zz;

	//Check the diagonal
	if(tr > 0.0f)
		{
		s = (float) sqrt(tr+1.0f);
		w = s/2.0f;
		s = 0.5f/s;
		x = (mT.zy-mT.yz)*s;
		y = (mT.xz-mT.zx)*s;
		z = (mT.yx-mT.xy)*s;
		}
	else
		{
		//Diagonal is negative
		i = 0;
		if(mT.yy > mT.xx)
			i = 1;
		if(mT.zz > mT.m44[i][i])
			i = 2;
		
		j = nxt[i];
		k = nxt[j];

		s = (float) sqrt((mT.m44[i][i]-(mT.m44[j][j]+mT.m44[k][k]))+1.0f);

		q[i] = s*0.5f;

		if(s)
			s = 0.5f/s;

		q[3] = (mT.m44[j][k] - mT.m44[k][j])*s;
		q[j] = (mT.m44[i][j] + mT.m44[j][i])*s;
		q[k] = (mT.m44[i][k] + mT.m44[k][i])*s;

		x = q[0];
		y = q[1];
		z = q[2];
		w = q[3];
		}
	}

void QUATERNION::GetAxisAngle(float &axisX, float &axisY, float &axisZ, float &rotAngle)
	{
	float lenOfVector = q[1]*q[1]+q[2]*q[2]+q[3]*q[3];

	if(lenOfVector < EPSILON)
		{
		axisX = 1.0f;
		axisY = 0.0f;
		axisZ = 0.0f;
		rotAngle = 0.0f;
		}
	else
		{
		float invLen = (float) (1.0/sqrt(lenOfVector));
		axisX = q[1]*invLen;
		axisY = q[2]*invLen;
		axisZ = q[3]*invLen;
		rotAngle = (float) (2.0f*acos(q[0]));
		}
	}

void QUATERNION::GetDirectionVector(VECTOR3D *dir)
	{
	Normalize();

	(*dir).x = 2.0f*(x*z-w*y);
	(*dir).y = 2.0f*(y*z+w*x);
	(*dir).z = 1.0f-2.0f*(x*x+y*y);
	}

void QUATERNION::EulerToQuat(const float exT, const float eyT, const float ezT)
	{
	double ex, ey, ez; //Temp half euler angles
	double cr, cp, cy, sr, sp, sy, cpcy, spsy; //Temp vars in roll, pitch and yaw

	//Convert to rads and half them
	ex = PIOVER180*exT/2.0f;
	ey = PIOVER180*eyT/2.0f;
	ez = PIOVER180*ezT/2.0f;

	cr = cos(ex);
	cp = cos(ey);
	cy = cos(ez);

	sr = sin(ex);
	sp = sin(ey);
	sy = sin(ez);

	cpcy = cp*cy;
	spsy = sp*sy;

	w = float(cr*cpcy + sr*spsy);

	x = float(sr*cpcy - cr*spsy);
	y = float(cr*sp*sy + sr*cp*sy);
	z = float(cr*cp*sy - sr*sp*cy);

	Normalize();
	}

void QUATERNION::GetEulerAngles(float &xT, float &yT, float &zT)
	{
	MATRIX4X4 mT;

	GetMatrix(&mT);
	mT.GetEulerAngles(xT, yT, zT);
	}

//	fTime = -2 * (x*x*x) + 3 * (x*x);
//	t = 0.5 - 0.5 * cos(sin(x*PI)*PI);
// Computes spherical linear interpolation between q1 and q2 with time 0-1
void QUATERNION::Slerp(const QUATERNION q1, const QUATERNION q2, const float fTime)
	{
	float omega, cosom, sinom, scale0, scale1;
	float from[4], to[4], to1[4];

	memcpy(&from, q1.q, sizeof(float)*4);
	memcpy(&to, q2.q, sizeof(float)*4);

	enum {W, X, Y, Z};

	//Calc cosine
	cosom = from[X]*to[X] + from[Y]*to[Y] + from[Z]*to[Z] + from[W]*to[W];

	//Adjust signs (if necessary)
	if(cosom < 0.0f)
		{
		  cosom = -cosom; 
		  to1[X] = -to[X];
          to1[Y] = -to[Y];
          to1[Z] = -to[Z];
          to1[W] = -to[W];
		}
	else 
		{
          to1[X] = to[X];
          to1[Y] = to[Y];
          to1[Z] = to[Z];
          to1[W] = to[W];
		}

	//Calculate coefficients
	if((1.0f-cosom) > EPSILON)
		{
		//Standard case (slerp)
		omega = (float) acos(cosom);
        sinom = (float) sin(omega);
        scale0 = (float) sin((1.0f-fTime)*omega)/sinom;
        scale1 = (float) sin(fTime*omega)/sinom;
		} 
	else
		{ 
		//"from" and "to" quaternions are very close 
		//... so we can do a linear interpolation
		scale0 = 1.0f-fTime;
        scale1 = fTime;
		}

	//Calculate final values
	q[X] = scale0*from[X] + scale1*to1[X];
	q[Y] = scale0*from[Y] + scale1*to1[Y];
	q[Z] = scale0*from[Z] + scale1*to1[Z];
	q[W] = scale0*from[W] + scale1*to1[W];
	}

void QUATERNION::SquadSlerp(QUATERNION aT, QUATERNION pT, QUATERNION qT,
							QUATERNION bT, float t)
	{
	QUATERNION ab;
	QUATERNION pq;

	ab.Slerp(aT, bT, t);
	pq.Slerp(pT, qT, t);
	Slerp(ab, pq, 2*t*(1-t));
	}

void QUATERNION::Invert(void)
	{
	float l, m;

	l = GetLength();
	if(fabs(l) < EPSILON)
		{
		x = y = z = 0.0f;
		w = 1.0f;
		}
	else
		{  
		m = 1.0f/l;
		x = -x*m;
		y = -y*m;
		z = -z*m;
		w = w*m;
		}
	}

void QUATERNION::Mult(QUATERNION a, QUATERNION b)
	{
	x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;
	y = a.w*b.y + a.y*b.w + a.z*b.x - a.x*b.z;
	z = a.w*b.z + a.z*b.w + a.x*b.y - a.y*b.x;
	w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
	}

void QUATERNION::Ln(void)
	{
	float om, s, t;

	s = (float) sqrt(x*x + y*y + z*z);
	om = (float) atan2(s, w);
	if(fabs(s) < EPSILON)
		t = 0.0f;
	else
		t = om/s;
	for(int i = 1; i < 4; ++i)
	  q[i] = q[i]*t;
	w = 0.0f;
	}

void QUATERNION::LnDif(QUATERNION a, QUATERNION b)
	{
	QUATERNION invp;

	for(int i = 0; i < 4; i++)
		invp.q[i] = a.q[i];
	invp.Invert();
	Mult(invp, b);
	Ln();
	}

void QUATERNION::Exp(void)
	{
	float om, sinom;

	om = (float) sqrt(x*x + y*y + z*z);
	if(fabs(om) < EPSILON)
		sinom = 1.0f;
	else
		sinom = (float) sin(om)/om;
	for(int i = 1; i < 4; ++i)
	  q[i] = q[i]*sinom;
	w = (float) cos(om);
	}

void QUATERNION::InnerPoint(QUATERNION pT, QUATERNION qT, QUATERNION nT)
	{
	QUATERNION dnT, dpT, xT;

	dnT.LnDif(qT, nT);
	dpT.LnDif(qT, pT);
	for(int i = 0; i < 4; i++)
		xT.q[i] = -1.0f/4.0f*(dnT.q[i]+dpT.q[i]);
	xT.Exp();
	Mult(qT, xT);
	}